#ifndef     __STEP_MOTOR_ACC_CTRL_H
#define     __STEP_MOTOR_ACC_CTRL_H

#include <stdint.h>

namespace utils {

/**
 * 0 ==> s0 加速
 * s0==> s1 匀速
 * s1==> total 减速
 */
template <typename MOTOR, uint32_t BASE_FREQ, uint32_t ACC, uint32_t VAR_RANGE>
class StepMotorAccCtrl {

public:
    void pwm_epoch_update() {
        m_current ++;

        if (m_current == m_total) {
            disable_pwm_self();
            notify_self();    
            return;
        }

        if (m_current <= m_s0) { // 加速
            set_pwm_freq_self(BASE_FREQ + m_current * ACC);
            return;
        }
        if (m_current <= m_s1) { // 匀速
            return;
        }

        set_pwm_freq_self(BASE_FREQ + (m_total - m_current) * ACC);
    }

protected:
    void run_motor(uint32_t total) {
        if (total < (VAR_RANGE * 2)) {
            m_s0 = total / 2;
        } else {
            m_s0 = VAR_RANGE;
        }
        m_s1 = total - VAR_RANGE;
        m_total = total;
        m_current = 0;
        enable_pwm_self(BASE_FREQ);
    }

private:
    inline void disable_pwm_self() {
        static_cast<MOTOR *>(this)->disable_pwm();
    }

    inline void notify_self() {
        static_cast<MOTOR *>(this)->notify();
    }

    inline void set_pwm_freq_self(uint32_t freq) {
        static_cast<MOTOR *>(this)->set_pwm_freq(freq);
    }

    inline void enable_pwm_self(uint32_t freq) {
        static_cast<MOTOR *>(this)->enable_pwm(freq);
    }

private:
    uint32_t m_total;
    uint32_t m_current;
    uint32_t m_s0;
    uint32_t m_s1;
};


}



#endif
